﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using NIP.ControllerInterface;

namespace NIP.DynamicControl.Environ
{
    public class PathSection
    {
        private PathPoint startPos;
        private PathPoint endPos;
        //public DateTime startTime;
        //public DateTime endTime;
        private double startTime;
        private int order;
        private double wristStep;
        private double[] a, b, c, d, e, f;
        private double[] velI, velF, accI, accF;
        private char endAction;
        public ControlCommands command;
        
        private double tf, tf2, tf3, tf4, tf5;



        public PathSection()
        {
            startPos = new PathPoint();
            endPos = new PathPoint();
            //startTime = new DateTime();
            command = ControlCommands.NULL;
            order = 5;
            tf = 1;
            tf2 = 1;
            tf3 = 1;
            tf4 = 1;
            tf5 = 1;
            wristStep = 0;
            velI = new double[3];
            velF = new double[3];
            accI = new double[3];
            accF = new double[3];
            a = new double[3];
            b = new double[3];
            c = new double[3];
            d = new double[3];
            e = new double[3];
            f = new double[3];
            Debug.WriteLine("Default Path Section created");
        }

        public PathSection(ControlCommands _command, int ord, PathPoint startP, PathPoint endP, double startT, double dur_sec, double[,] vaInitial, double[,] vaFinal, char act)
        {
            //4th
            //[ 1, 0, 0, 0, 0,                                                s0]
            //[ 0, 1, 0, 0, 0,                                                v0]
            //[ 0, 0, 1, 0, 0,                                              a0/2]
            //[ 0, 0, 0, 1, 0,    -(a0*tf^2 + (3*v0 + vf)*tf + 4*s0 - 4*sf)/tf^3]
            //[ 0, 0, 0, 0, 1, ((a0*tf^2)/2 + (2*v0 + vf)*tf + 3*s0 - 3*sf)/tf^4]

            //5th
            //[ 1, 0, 0, 0, 0, 0,                                                                s0]
            //[ 0, 1, 0, 0, 0, 0,                                                                v0]
            //[ 0, 0, 1, 0, 0, 0,                                                              a0/2]
            //[ 0, 0, 0, 1, 0, 0, -(((3*a0)/2 - af/2)*tf^2 + (6*v0 + 4*vf)*tf + 10*s0 - 10*sf)/tf^3]
            //[ 0, 0, 0, 0, 1, 0,    (((3*a0)/2 - af)*tf^2 + (8*v0 + 7*vf)*tf + 15*s0 - 15*sf)/tf^4]
            //[ 0, 0, 0, 0, 0, 1,       -((a0/2 - af/2)*tf^2 + (3*v0 + 3*vf)*tf + 6*s0 - 6*sf)/tf^5]
            command = _command;
            endAction = act;

            order = ord;
            startPos = startP;
            endPos = endP;
            startTime = startT;
            //endTime = startTime;
            //endTime.AddSeconds(dur_sec);
            tf = dur_sec;
            tf2 = tf * tf;
            tf3 = tf2 * tf;
            tf4 = tf3 * tf;
            tf5 = tf4 * tf;
            velI = new double[3];
            velF = new double[3];
            accI = new double[3];
            accF = new double[3];
            a = new double[3];
            b = new double[3];
            c = new double[3];
            d = new double[3];
            e = new double[3];
            f = new double[3];
            velI[0] = vaInitial[0,0]; 
            velI[1] = vaInitial[0,1];
            velI[2] = vaInitial[0,2]; 
            accI[0] = vaInitial[1,0]; 
            accI[1] = vaInitial[1,1]; 
            accI[2] = vaInitial[1,2];
            velF[0] = vaFinal[0,0]; 
            velF[1] = vaFinal[0,1]; 
            velF[2] = vaFinal[0,2];
            wristStep = (endP.WristPitch - startP.WristPitch) / dur_sec;
            a[0] = startPos.pos[0];
            a[1] = startPos.pos[1];
            a[2] = startPos.pos[2];

            b[0] = velI[0];
            b[1] = velI[1];
            b[2] = velI[2];

            c[0] = accI[0] / 2.0;
            c[1] = accI[1] / 2.0;
            c[2] = accI[2] / 2.0;

            if (order == 5)
            {
                accF[0] = vaFinal[1, 0];
                accF[1] = vaFinal[1, 1];
                accF[2] = vaFinal[1, 2];

                // -(((3*a0)/2 - af/2)*tf^2 + (6*v0 + 4*vf)*tf + 10*s0 - 10*sf)/tf^3
                d[0] = -((((3.0 * accI[0]) - accF[0]) / 2.0) * tf2 + (6.0 * velI[0] + 4.0 * velF[0]) * tf + 10.0 * (a[0] - endPos.pos[0])) / tf3;
                d[1] = -((((3.0 * accI[1]) - accF[1]) / 2.0) * tf2 + (6.0 * velI[1] + 4.0 * velF[1]) * tf + 10.0 * (a[1] - endPos.pos[1])) / tf3;
                d[2] = -((((3.0 * accI[2]) - accF[2]) / 2.0) * tf2 + (6.0 * velI[2] + 4.0 * velF[2]) * tf + 10.0 * (a[2] - endPos.pos[2])) / tf3;

                // (((3*a0)/2 - af)*tf^2 + (8*v0 + 7*vf)*tf + 15*s0 - 15*sf)/tf^4
                e[0] = (((3.0 * accI[0]) / 2.0 - accF[0]) * tf2 + (8.0 * velI[0] + 7.0 * velF[0]) * tf + 15.0 * (a[0] - endPos.pos[0])) / tf4;
                e[1] = (((3.0 * accI[1]) / 2.0 - accF[1]) * tf2 + (8.0 * velI[1] + 7.0 * velF[1]) * tf + 15.0 * (a[1] - endPos.pos[1])) / tf4;
                e[2] = (((3.0 * accI[2]) / 2.0 - accF[2]) * tf2 + (8.0 * velI[2] + 7.0 * velF[2]) * tf + 15.0 * (a[2] - endPos.pos[2])) / tf4;

                // -((a0/2 - af/2)*tf^2 + (3*v0 + 3*vf)*tf + 6*s0 - 6*sf)/tf^5
                f[0] = -(((accI[0] - accF[0]) / 2.0) * tf2 + 3.0 * (velI[0] + velF[0]) * tf + 6.0 * (a[0] - endPos.pos[0])) / tf5;
                f[1] = -(((accI[1] - accF[1]) / 2.0) * tf2 + 3.0 * (velI[1] + velF[1]) * tf + 6.0 * (a[1] - endPos.pos[1])) / tf5;
                f[2] = -(((accI[2] - accF[2]) / 2.0) * tf2 + 3.0 * (velI[2] + velF[2]) * tf + 6.0 * (a[2] - endPos.pos[2])) / tf5;
            }
            else //4th order
            {
                d[0] = -(accI[0] * tf2 + (3 * velI[0] + velF[0]) * tf + 4 * a[0] - 4 * endPos.pos[0]) / tf3;
                d[1] = -(accI[1] * tf2 + (3 * velI[1] + velF[1]) * tf + 4 * a[1] - 4 * endPos.pos[1]) / tf3;
                d[2] = -(accI[2] * tf2 + (3 * velI[2] + velF[2]) * tf + 4 * a[2] - 4 * endPos.pos[2]) / tf3;

                e[0] = ((accI[0] * tf2) / 2 + (2 * velI[0] + velF[0]) * tf + 3 * a[0] - 3 * endPos.pos[0]) / tf4;
                e[1] = ((accI[1] * tf2) / 2 + (2 * velI[1] + velF[1]) * tf + 3 * a[1] - 3 * endPos.pos[1]) / tf4;
                e[2] = ((accI[2] * tf2) / 2 + (2 * velI[2] + velF[2]) * tf + 3 * a[2] - 3 * endPos.pos[2]) / tf4;

                accF[0] = (2.0*c[0]) + (6.0*d[0])*tf + (12.0*e[0]*tf2);
                accF[1] = (2.0*c[1]) + (6.0*d[1])*tf + (12.0*e[1]*tf2);
                accF[2] = (2.0*c[2]) + (6.0*d[2])*tf + (12.0*e[2]*tf2);
            }
        }

        public PathSection(ControlCommands _command, int ord, PathPoint startP, PathPoint endP, double startT, double dur_sec, char act)
        {
            //4th
            //[ 1, 0, 0, 0, 0,                                                s0]
            //[ 0, 1, 0, 0, 0,                                                v0]
            //[ 0, 0, 1, 0, 0,                                              a0/2]
            //[ 0, 0, 0, 1, 0,    -(a0*tf^2 + (3*v0 + vf)*tf + 4*s0 - 4*sf)/tf^3]
            //[ 0, 0, 0, 0, 1, ((a0*tf^2)/2 + (2*v0 + vf)*tf + 3*s0 - 3*sf)/tf^4]

            //5th
            //[ 1, 0, 0, 0, 0, 0,                                                                s0]
            //[ 0, 1, 0, 0, 0, 0,                                                                v0]
            //[ 0, 0, 1, 0, 0, 0,                                                              a0/2]
            //[ 0, 0, 0, 1, 0, 0, -(((3*a0)/2 - af/2)*tf^2 + (6*v0 + 4*vf)*tf + 10*s0 - 10*sf)/tf^3]
            //[ 0, 0, 0, 0, 1, 0,    (((3*a0)/2 - af)*tf^2 + (8*v0 + 7*vf)*tf + 15*s0 - 15*sf)/tf^4]
            //[ 0, 0, 0, 0, 0, 1,       -((a0/2 - af/2)*tf^2 + (3*v0 + 3*vf)*tf + 6*s0 - 6*sf)/tf^5]
            command = _command;
            order = ord;
            startPos = startP;
            endPos = endP;
            startTime = startT;
            //endTime = startTime;
            //endTime.AddSeconds(dur_sec);
            tf = dur_sec;
            tf2 = tf * tf;
            tf3 = tf2 * tf;
            tf4 = tf3 * tf;
            tf5 = tf4 * tf;
            velI = new double[3];
            velF = new double[3];
            accI = new double[3];
            accF = new double[3];
            a = new double[3];
            b = new double[3];
            c = new double[3];
            d = new double[3];
            e = new double[3];
            f = new double[3];
            velI[0] = startPos.vel[0];
            velI[1] = startPos.vel[1];
            velI[2] = startPos.vel[2];
            accI[0] = startPos.acc[0];
            accI[1] = startPos.acc[1];
            accI[2] = startPos.acc[2];
            velF[0] = endPos.vel[0];
            velF[1] = endPos.vel[1];
            velF[2] = endPos.vel[2]; 
            wristStep = (endP.WristPitch - startP.WristPitch) / dur_sec;
            a[0] = startPos.pos[0];
            a[1] = startPos.pos[1];
            a[2] = startPos.pos[2];

            b[0] = velI[0];
            b[1] = velI[1];
            b[2] = velI[2];

            c[0] = accI[0] / 2.0;
            c[1] = accI[1] / 2.0;
            c[2] = accI[2] / 2.0;

            if (order == 5)
            {
                accF[0] = endPos.acc[0];
                accF[1] = endPos.acc[1];
                accF[2] = endPos.acc[2];

                // -(((3*a0)/2 - af/2)*tf^2 + (6*v0 + 4*vf)*tf + 10*s0 - 10*sf)/tf^3
                d[0] = -((((3.0 * accI[0]) - accF[0]) * 0.5 * tf2) + ((6.0 * velI[0] + 4.0 * velF[0]) * tf) + (10.0 * (a[0] - endPos.pos[0]))) / tf3;
                d[1] = -((((3.0 * accI[1]) - accF[1]) * 0.5 * tf2) + ((6.0 * velI[1] + 4.0 * velF[1]) * tf) + (10.0 * (a[1] - endPos.pos[1]))) / tf3;
                d[2] = -((((3.0 * accI[2]) - accF[2]) * 0.5 * tf2) + ((6.0 * velI[2] + 4.0 * velF[2]) * tf) + (10.0 * (a[2] - endPos.pos[2]))) / tf3;

                // (((3*a0)/2 - af)*tf^2 + (8*v0 + 7*vf)*tf + 15*s0 - 15*sf)/tf^4
                e[0] = ((((3.0 * accI[0]) * 0.5 - accF[0]) * tf2) + ((8.0 * velI[0] + 7.0 * velF[0]) * tf) + (15.0 * (a[0] - endPos.pos[0]))) / tf4;
                e[1] = ((((3.0 * accI[1]) * 0.5 - accF[1]) * tf2) + ((8.0 * velI[1] + 7.0 * velF[1]) * tf) + (15.0 * (a[1] - endPos.pos[1]))) / tf4;
                e[2] = ((((3.0 * accI[2]) * 0.5 - accF[2]) * tf2) + ((8.0 * velI[2] + 7.0 * velF[2]) * tf) + (15.0 * (a[2] - endPos.pos[2]))) / tf4;

                // -((a0/2 - af/2)*tf^2 + (3*v0 + 3*vf)*tf + 6*s0 - 6*sf)/tf^5
                f[0] = -((((accI[0] - accF[0]) * 0.5) * tf2) + (3.0 * (velI[0] + velF[0]) * tf) + (6.0 * (a[0] - endPos.pos[0]))) / tf5;
                f[1] = -((((accI[1] - accF[1]) * 0.5) * tf2) + (3.0 * (velI[1] + velF[1]) * tf) + (6.0 * (a[1] - endPos.pos[1]))) / tf5;
                f[2] = -((((accI[2] - accF[2]) * 0.5) * tf2) + (3.0 * (velI[2] + velF[2]) * tf) + (6.0 * (a[2] - endPos.pos[2]))) / tf5;
            }
            else //4th order
            {
                d[0] = -((accI[0] * tf2) + ((3.0 * velI[0] + velF[0]) * tf) + (4.0 * (a[0] - endPos.pos[0]))) / tf3;
                d[1] = -((accI[1] * tf2) + ((3.0 * velI[1] + velF[1]) * tf) + (4.0 * (a[1] - endPos.pos[1]))) / tf3;
                d[2] = -((accI[2] * tf2) + ((3.0 * velI[2] + velF[2]) * tf) + (4.0 * (a[2] - endPos.pos[2]))) / tf3;

                e[0] = ((accI[0] * tf2 * 0.5) + ((2.0 * velI[0] + velF[0]) * tf) + (3.0 * (a[0] - endPos.pos[0]))) / tf4;
                e[1] = ((accI[1] * tf2 * 0.5) + ((2.0 * velI[1] + velF[1]) * tf) + (3.0 * (a[1] - endPos.pos[1]))) / tf4;
                e[2] = ((accI[2] * tf2 * 0.5) + ((2.0 * velI[2] + velF[2]) * tf) + (3.0 * (a[2] - endPos.pos[2]))) / tf4;

                endPos.acc[0] = (2.0 * c[0]) + (6.0 * d[0]) * tf + (12.0 * e[0] * tf2);
                endPos.acc[1] = (2.0 * c[1]) + (6.0 * d[1]) * tf + (12.0 * e[1] * tf2);
                endPos.acc[2] = (2.0 * c[2]) + (6.0 * d[2]) * tf + (12.0 * e[2] * tf2);
            }
        }

        public PathPoint getEndPos()
        {
            return endPos;
        }

        public PathSection(PathSection ps)
        {
            command = ps.command;
            startPos = ps.startPos;
            endPos = ps.endPos;
            startTime = ps.startTime;
            //endTime = ps.endTime;
            tf = ps.tf;
            tf2 = ps.tf2;
            tf3 = ps.tf3;
            tf4 = ps.tf4;
            tf5 = ps.tf5;
            for (int i = 0; i < 3; i++)
            {
                velI[i] = ps.velI[i];
                velF[i] = ps.velF[i];
                accI[i] = ps.accI[i];
                accF[i] = ps.accF[i];
                a[i] = ps.a[i];
                b[i] = ps.b[i];
                c[i] = ps.c[i];
                d[i] = ps.d[i];
                e[i] = ps.e[i];
                f[i] = ps.f[i];
            }
        }


        public double getDuration()
        {
            return tf;
        }

        public double getStartTime()
        {
            return startTime;
        }

        public double getEndTime()
        {
            return startTime + tf;
        }

        public ControlCommands getCommand()
        {
            return command;
        }
        
        private double[] getPos(double t)
        {
            double t2 = t * t;
            double t3 = t2 * t;
            double t4 = t3 * t;
            double t5 = t4 * t;
            double[] pos = new double[3];
            for (int i = 0; i < 3; i++)
                pos[i] = a[i] + b[i] * t + c[i] * t2 + d[i] * t3 + e[i] * t4 + f[i] * t5;
            return pos;
        }

        private double[] getVel(double t)
        {
            double t2 = t * t;
            double t3 = t2 * t;
            double t4 = t3 * t;
            double[] vel = new double[3];
            for (int i = 0; i < 3; i++)
                vel[i] = b[i] + 2.0 * c[i] * t + 3.0 * d[i] * t2 + 4.0 * e[i] * t3 + 5.0 * f[i] * t4;
            return vel;
        }

        private double[] getAcc(double t)
        {
            double t2 = t * t;
            double t3 = t2 * t;
            double[] acc = new double[3];
            for (int i = 0; i < 3; i++)
                acc[i] = 2.0 * c[i] + 6.0 * d[i] * t + 12.0 * e[i] * t2 + 20.0 * f[i] * t3;
            return acc;
        }

        public char getEndAction()
        {
            return endAction;
        }

        public PathPoint solveForJointAngles(double t_des)
        {
            //DateTime currentTime = DateTime.Now;
            PathPoint returnPos = new PathPoint();
            double sectionT = t_des - startTime;
            double t = ((sectionT > tf) ? tf : sectionT);
            for(int j = 0; j < 5; j++)
            {
                returnPos.setJoint(j, (startPos.getJoint(j)+((endPos.getJoint(j) - startPos.getJoint(j)) * (t/tf))));

            }
            
            return returnPos;
        }
        
        public PathPoint solveForPosition(double t_des)
        {
            //DateTime currentTime = DateTime.Now;
            PathPoint returnPos = new PathPoint();
            double sectionT = t_des - startTime;
            double t = ((sectionT > tf) ? tf : sectionT);
            double t2 = t * t;
            double t3 = t2 * t;
            double t4 = t3 * t;
            double t5 = t4 * t;
            if (order == 5)
            {
                for (int i = 0; i < 3; i++)
                {
                    returnPos.pos[i] = a[i] + (b[i] * t) + (c[i] * t2) + (d[i] * t3) + (e[i] * t4) + (f[i] * t5);
                    returnPos.vel[i] = b[i] + (2.0 * c[i] * t) + (3.0 * d[i] * t2) + (4.0 * e[i] * t3) + (5.0 * f[i] * t4);
                    returnPos.acc[i] = (2.0 * c[i]) + (6.0 * d[i] * t) + (12.0 * e[i] * t2) + (20.0 * f[i] * t3);
                }
            }
            else
            {
                for (int i = 0; i < 3; i++)
                {
                    returnPos.pos[i] = a[i] + (b[i] * t) + (c[i] * t2) + (d[i] * t3) + (e[i] * t4);
                    returnPos.vel[i] = b[i] + (2.0 * c[i] * t) + (3.0 * d[i] * t2) + (4.0 * e[i] * t3);
                    returnPos.acc[i] = (2.0 * c[i]) + (6.0 * d[i] * t) + (12.0 * e[i] * t2);
                }
            }
            returnPos.WristPitch = startPos.WristPitch + (wristStep * t);
            return returnPos;
        }




    }
}
